/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : control.cpp
  * Description        :
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Oct 23, 2020
  * Co-Author          : Jiapeng Chen
  * Creation Date      : May 12, 2023
  ******************************************************************************
  */
/* USER CODE END Header */

#include "control.hpp"

PID pid[PID_NUM];
PID *pidRolRate=&pid[0],*pidPitRate=&pid[1],*pidYawRate=&pid[2];//角速度
PID *pidRol=&pid[3],*pidPit=&pid[4],*pidYaw=&pid[5];//角度
PID *pidU=&pid[6],*pidV=&pid[7],*pidW=&pid[8];//速度
PID *pidX=&pid[9],*pidY=&pid[10],*pidZ=&pid[11];//位置
PID *pidTHR=&pid[12],*pidTHRH=&pid[13];
//----自动起降油门PID控制


ATTITUDE_CTRL::ATTITUDE_CTRL()
{	//p i d b i e d
	//原参数
//	pidRolRate->Pid_Set_Para(1.3f, 0.14f, 0.08f, 0.5f, 10000*M_PI, 4*M_PI, M_PI);
//	pidPitRate->Pid_Set_Para(1.3f, 0.14f, 0.08f, 0.5f, 10000*M_PI, 4*M_PI, M_PI);
//	pidYawRate->Pid_Set_Para(1.3f, 0.14f, 0.08f, 0.0f, 10000*M_PI, 4*M_PI, M_PI);
//	pidRol->Pid_Set_Para(4.8f, 0.0f, 2.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
//	pidPit->Pid_Set_Para(4.8f, 0.0f, 2.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
//	pidYaw->Pid_Set_Para(4.5f, 0.0f, 2.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
//	pidU->Pid_Set_Para(20.0f, 0.0f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
//	pidV->Pid_Set_Para(20.0f, 0.0f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
//	pidW->Pid_Set_Para(40.0f, 8.0f, 0.2f, 0.5f, 100*M_PI, 4*M_PI, M_PI);
//	pidX->Pid_Set_Para(1.0f, 0.0f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
//	pidY->Pid_Set_Para(1.0f, 0.0f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
//	pidZ->Pid_Set_Para(1.6f, 0.1f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);

	//欧康祺的参数
//	pidRolRate->Pid_Set_Para(2.5f, 0.1f, 0.05f, 1.0f, 1.5*M_PI, 0.56*M_PI, 0.56*M_PI);
//	pidPitRate->Pid_Set_Para(2.5f, 0.1f, 0.05f, 1.0f, 1.5*M_PI, 0.56*M_PI, 0.56*M_PI);
//	pidYawRate->Pid_Set_Para(2.5f, 0.0f, 0.00f, 1.0f, 1.5*M_PI, 0.56*M_PI, 0.56*M_PI);
//	pidRol->Pid_Set_Para(4.8f, 0.0f, 0.0f, 0.0f, 0.5*M_PI, 0.5*M_PI, 0.5*M_PI);
//	pidPit->Pid_Set_Para(4.8f, 0.0f, 0.0f, 0.0f, 0.5*M_PI, 0.5*M_PI, 0.5*M_PI);
//	pidYaw->Pid_Set_Para(4.6f, 0.0f, 0.0f, 0.0f, 0.5*M_PI, 0.5*M_PI, 0.5*M_PI);
	switch(plane_num)
	{
		case 0:
			//23.9.04大飞机P0参数
			pidRolRate->Pid_Set_Para(1.0f, 0.01f, 0.00f, 1.0f, 100*M_PI, 10*M_PI, M_PI);
			pidPitRate->Pid_Set_Para(4.0f, 0.08f, 0.00f, 1.0f, 100*M_PI, 10*M_PI, M_PI);
			pidYawRate->Pid_Set_Para(6.0f, 0.06f, 0.00f, 1.0f, 100*M_PI, 10*M_PI, M_PI);
			pidRol->Pid_Set_Para(9.8f, 0.04f, 0.01f, 0.0f, 200*M_PI, 10*M_PI, M_PI);
			pidPit->Pid_Set_Para(15.4f, 0.05f, 0.01f, 0.0f, 200*M_PI, 10*M_PI, M_PI);
			pidYaw->Pid_Set_Para(7.2f, 0.20f, 0.00f, 0.0f, 200*M_PI, 6*M_PI, M_PI);

			pidU->Pid_Set_Para(8.4f, 0.02f, 0.00f, 0.0f, 20000, 20*M_PI, M_PI);    //ROL
			pidV->Pid_Set_Para(7.2f, 0.02f, 0.00f, 0.0f, 20000, 20*M_PI, M_PI);     //PIT
			pidW->Pid_Set_Para(6.8f, 0.08f, 0.00f, 0.0f, 10000, 8*M_PI, M_PI);    //Z_OUT
			pidX->Pid_Set_Para(0.4f, 0.01f, 0.00f, 0.0f, 100*M_PI, 8*M_PI, M_PI);
			pidY->Pid_Set_Para(0.8f, 0.01f, 0.00f, 0.0f, 100*M_PI, 8*M_PI, M_PI);
			pidZ->Pid_Set_Para(2.8f, 0.10f, 0.00f, 0.0f, 100*M_PI, 8*M_PI, M_PI);  //SetUvw[2]
			break;
		case 1:
			//23.2.25小飞机P1参数
			pidRolRate->Pid_Set_Para(0.4f, 0.00f, 0.01f, 1.0f, 100*M_PI, 4*M_PI, M_PI);
			pidPitRate->Pid_Set_Para(1.0f, 0.00f, 0.01f, 1.0f, 100*M_PI, 4*M_PI, M_PI);
			pidYawRate->Pid_Set_Para(6.0f, 0.00f, 0.00f, 1.0f, 100*M_PI, 4*M_PI, M_PI);
			pidRol->Pid_Set_Para(5.8f, 0.01f, 0.04f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
			pidPit->Pid_Set_Para(12.8f, 0.01f, 0.14f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
			pidYaw->Pid_Set_Para(22.8f, 0.01f, 0.01f, 0.0f, 100*M_PI, 4*M_PI, M_PI);

			pidU->Pid_Set_Para(4.3f, 0.01f, 0.01f, 0.0f, 10000, 4*M_PI, M_PI);    //ROL
			pidV->Pid_Set_Para(4.4f, 0.01f, 0.01f, 0.0f, 10000, 4*M_PI, M_PI);   //PIT
			pidW->Pid_Set_Para(8.2f, 0.01f, 0.48f, 0.0f, 10000, 4*M_PI, M_PI);    //Z_OUT
			pidX->Pid_Set_Para(0.2f, 0.00f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
			pidY->Pid_Set_Para(0.2f, 0.00f, 0.0f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
			pidZ->Pid_Set_Para(8.2f, 0.03f, 0.32f, 0.0f, 100*M_PI, 4*M_PI, M_PI);  //SetUvw[2]
			break;
		case 2:
			//23.9.04小飞机P2参数
			pidRolRate->Pid_Set_Para(1.5f, 0.01f, 0.00f, 1.0f, 100*M_PI, 10*M_PI, M_PI);
			pidPitRate->Pid_Set_Para(2.8f, 0.04f, 0.00f, 1.0f, 100*M_PI, 10*M_PI, M_PI);
			pidYawRate->Pid_Set_Para(9.8f, 0.08f, 0.00f, 1.0f, 100*M_PI, 4*M_PI, M_PI);
			pidRol->Pid_Set_Para(8.0f, 0.02f, 0.00f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
			pidPit->Pid_Set_Para(8.6f, 0.06f, 0.00f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
			pidYaw->Pid_Set_Para(5.0f, 0.26f, 0.00f, 0.0f, 100*M_PI, 4*M_PI, M_PI);

			pidU->Pid_Set_Para(8.6f, 0.04f, 0.00f, 0.0f, 10000, 4*M_PI, M_PI);    //ROL
			pidV->Pid_Set_Para(8.2f, 0.00f, 0.00f, 0.0f, 10000, 4*M_PI, M_PI);     //PIT
			pidW->Pid_Set_Para(15.5f, 0.20f, 0.00f, 0.0f, 10000, 4*M_PI, M_PI);    //Z_OUT
			pidX->Pid_Set_Para(0.8f, 0.03f, 0.00f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
			pidY->Pid_Set_Para(0.6f, 0.01f, 0.00f, 0.0f, 100*M_PI, 4*M_PI, M_PI);
			pidZ->Pid_Set_Para(5.8f, 0.40f, 0.00f, 0.0f, 100*M_PI, 4*M_PI, M_PI);  //SetUvw[2]

			break;
	}
}

ATTITUDE_CTRL::~ATTITUDE_CTRL()
{

}

void ATTITUDE_CTRL::flight_Mode_Update()
{
	/* 判别飞行模式 ******************************************/
	switch(rcCommand.Key[0])
	{
		case 0: //姿态控制
			Mode = QAUD_ATTITUDE;
			break;
		case 1: //定速控制
			Mode = QAUD_SPEED;
			break;
		case 2: //位置控制
			Mode = QAUD_POS_HOLD;
			break;
	}
	if(rcCommand.Key[2] < 2)	//锁尾
	{
		LockHead = true;
		if(LockHeadFirst)
		{
			HoldYaw = ahrsEuler.Ang[2]*R2D;
			LockHeadFirst = false;
		}
	}
	else
	{
		LockHead = false;
		LockHeadFirst = true;
	}
}

void ATTITUDE_CTRL::motor_Output()
{
	if (control == false)
	{
		motorPWM.PWM[0] = 0; //FRONT_L 前左电机
		motorPWM.PWM[1] = 0; //REAR_L  后左电机
		motorPWM.PWM[2] = 0; //FRONT_R 前右电机
		motorPWM.PWM[3] = 0; //REAR_R  后右电机
	}
	else if (control == true)
	{
		if(THROTTLE < PWM_IDLE)
		{
			#ifdef QUADX	//X字型的四轴飞行器
				motorPWM.PWM[0] = PWM_IDLE; //FRONT_L 前左电机
				motorPWM.PWM[1] = PWM_IDLE; //REAR_L  后左电机
				motorPWM.PWM[2] = PWM_IDLE; //FRONT_R 前右电机
				motorPWM.PWM[3] = PWM_IDLE; //REAR_R  后右电机
			#endif
		}
		else
		{
			#define MIX(X,Y,Z) THROTTLE + ROL_OUT*(X) + PIT_OUT*(Y) + YAW_OUT*(Z) + Z_OUT	//  FOREWARD + Z_OUT +

			#ifdef QUADP	//十字型的四轴飞行器
				motorPWM.PWM[4] = MIX( 0,+1,+1); //FRONT 前面电机
				motorPWM.PWM[5] = MIX(+1, 0,-1); //LEFT	 左边电机
				motorPWM.PWM[6] = MIX( 0,-1,+1); //REAR	 后尾电机
				motorPWM.PWM[7] = MIX(-1, 0,-1); //RIGHT 右边电机
			#endif
			#ifdef QUADX	//X字型的四轴飞行器，以longmen标志为前方  偏航逆时针：13减速，24加速
				motorPWM.PWM[0] = MIX(-1,+1,-1); //FRONT_R 前右电机 M1 顺//4
				motorPWM.PWM[1] = MIX(-1,-1,+1); //REAR_R  后右电机 M2 逆//3
				motorPWM.PWM[2] = MIX(+1,-1,-1); //REAR_L  后左电机 M3 顺//1
				motorPWM.PWM[3] = MIX(+1,+1,+1); //FRONT_L 前左电机 M4 逆//2
			#endif
			#ifdef OCTAP	//十字型的八轴飞行器
				const float rt2 = 1.414213562373f;
				motorPWM.PWM[4] = MIX(   0,  +2,-1); //1号电机
				motorPWM.PWM[5] = MIX(-rt2, rt2,+1); //2号电机
				motorPWM.PWM[6] = MIX(  -2,   0,-1); //3号电机
				motorPWM.PWM[7] = MIX(-rt2,-rt2,+1); //4号电机
				motorPWM.PWM[0] = MIX(   0,  -2,-1); //5号电机
				motorPWM.PWM[1] = MIX( rt2,-rt2,+1); //6号电机
				motorPWM.PWM[2] = MIX(  +2,   0,-1); //7号电机
				motorPWM.PWM[3] = MIX( rt2, rt2,+1); //8号电机
			#endif
		}
	}

	xQueueOverwrite(queueMotorPWM,&motorPWM);
}

void ATTITUDE_CTRL::Attitude_Ctrl_Init()
{
	LockHeadFirst = true;
	isFirstAltHold = true;
	isFirstPosHold = true;
	Mode = QAUD_ATTITUDE;

	osDelay(100);
	for(uint8_t i=0;i<12;i++)
	{
		pid_para.kp[i] = pid[i].Pid_Get_Kp();
		pid_para.ki[i] = pid[i].Pid_Get_Ki();
		pid_para.kd[i] = pid[i].Pid_Get_Kd();
	}
	xQueueOverwrite(queuePID,&pid_para);
//
///////////////////////adding////////////////////////////////////////////////////////////////////////////////////////////////
//	for (int i = 0; i < 3; i++)
//	{
//		F[i] = 0.0f;
//		X1_[i] = 0.0f;
//		X2_[i] = 0.0f;
//		beta1[i] = 2.0f;
//		beta2[i] = 9.0f;
//		Wn[i] = 0.0f;
//		Wn_last[i] = 0.0f;
//		dWn[i] = 0.0f;
//		dWn_last[i] = 0.0f;
//		ddWn[i] = 0.0f;
//		ddWn_last[i] = 0.0f;
//		dddWn[i] = 0.0f;
//	}
//	J << 6.8086e-5f,0.0f,0.0f,
//				0.0f,7.85268e-5f,0.0f,
//				0.0f,0.0f,5.9534e-5f;
//	moments << 0.0f, 0.0f, 0.0f;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	for(int i=0;i<3;i++) pos_old[i] = 0;

	TakeOff_Count = 0;
	LandingHeight = 0;
	isFirstProcessPos = true;
	isFirstLanding = true;
	isFirstTrajectory = true;

	trajectory_starttime = 0;
	time_now = 0;
	trajectory_time = 0;
	trajectory_stoptime = 0;

}

void ATTITUDE_CTRL::Attitude_ctrl_run()
{

	getTimer_us(&startTimer);
	xQueuePeek(queueGyrDat, &gyro, 0);
	xQueuePeek(queueRCCommand, &rcCommand, 0);
	xQueuePeek(queueAhrsEuler, &ahrsEuler, 0);
	xQueuePeek(queueHeight, &height, 0);
	xQueuePeek(queueFlow, &flow, 0);
//	xQueuePeek(queueUwbPosVel,&ekf_uwb_pos_vel, 0);
	xQueuePeek(queueKalUwbPosVel, &kal_uwb_pos_vel, 0);
//	xQueuePeek(queueESKF,&odometry, 0);
//	xQueuePeek(queueMahonyAhrs, &mahonyAhrs_Ang, 0);
	xQueuePeek(queueGyrDatFil, &gyro_filter, 0); //陀螺仪传感器滤波后的数据
//	xQueuePeek(queueLanding,&LandingMsg, 0);
	xQueuePeek(queueMOCAP, &mocap, 0);
	xQueuePeek(queueUWB, &uwb, 0);

//	pid_Para_Update();
	pid_Para_Update_Time++;
	if(pid_Para_Update_Time>50)		//50个控制周期更新一次PID参数
	{
		pid_Para_Update();
		pid_Para_Update_Time = 0;
	}

	flight_Mode_Update();

//	if(rcCommand.Thr < 70.0f)
//	{
//		for(u8 i=0; i<PID_NUM; i++)
//		{
//			pid[i].Pid_Reset_2();
//		}
//	}

	if (rcCommand.Key[3] >= 2)
		control = true;
	else
		control = false;

//	if(control==true)
	{

		switch(Mode)
		{
		case QAUD_MANUAL:   //手控
				isFirstAltHold = true;

				SetDAng[0] = rcCommand.dAng[0];
				SetDAng[1] = rcCommand.dAng[1];
				if(LockHead == true)
				{
					SetAng[2] = rcCommand.Ang[2] + HoldYaw;//锁尾，航向控角度
					FeedBackAng[2] = ahrsEuler.Ang[2]*R2D;
					SetDAng[2] = pidYaw->Pid_Controller(SetAng[2], FeedBackAng[2],0.005f,0);
				}
				else
				{
					SetDAng[2] = rcCommand.dAng[2];//不锁尾，航向控角速度
				}
				THROTTLE = rcCommand.Thr;
				Z_OUT = 0;
#if 0
///////////////////////////////adding////////////////////////////////////////////////
				//当前角速度
				angleRate << gyro.gyro[0], gyro.gyro[1], gyro.gyro[2];
				angleRate_ << 0.0f, -gyro.gyro[2], gyro.gyro[1],
							  gyro.gyro[2], 0.0f, -gyro.gyro[0],
							  -gyro.gyro[1], gyro.gyro[0], 0.0f;

				F[0] = (J(1,1) - J(2,2)) * angleRate(1) * angleRate(2) / J(0,0);
				F[1] = (J(2,2) - J(0,0)) * angleRate(2) * angleRate(0) / J(1,1);
				F[2] = (J(0,0) - J(1,1)) * angleRate(0) * angleRate(1) / J(2,2);

				time += dt;

				for (int i=0; i<3; i++)
				{
					if(time <= 5) Wnc = 5;
					else Wnc = 15;

					dddWn[i] = -2 * Wb * ddWn_last[i] - 2 * Wb*Wb * dWn_last[i] - Wb*Wb*Wb * Wn[i] + Wb*Wb*Wb * Wnc;
					ddWn[i] = ddWn_last[i] + dddWn[i] * dt;
					dWn[i] = dWn_last[i] + ddWn[i] * dt;
					Wn[i] = Wn_last[i] + dWn[i] * dt;

					beta1[i] = abs(2.0f * Wn[i] - dWn[i] / Wn[i]);
					beta2[i] = abs(Wn[i]*Wn[i] + ddWn[i] / Wn[i] - 2.0f * dWn[i] - ((dWn[i]*dWn[i]) / (Wn[i]*Wn[i])));

					X1_[i] = X1_[i] + h * (F[i] + moments(i)/J(i,i) + X2_[i] + beta1[i] * (angleRate(i) - X1_[i]));
					X2_[i] = X2_[i] + h * beta2[i] * (angleRate(i) - X1_[i]);

					Wn_last[i] = Wn[i];
					dWn_last[i] = dWn[i];
					ddWn_last[i] = ddWn[i];
				}
				D << X2_[0], X2_[1], X2_[2];

				moments = J * D;

///////////////////////////////////////////////////////////////////////////////////
#endif
				break;
			//姿态控制
			case QAUD_ATTITUDE:
				if (isFirstAltHold == false)
				{
					isFirstAltHold = true;
					pidZ->Pid_Reset_2();
					pidW->Pid_Reset_2();
				}
				if (isFirstPosHold == false)
				{
					isFirstPosHold = true;
					pidX->Pid_Reset_2();
					pidY->Pid_Reset_2();
					pidU->Pid_Reset_2();
					pidV->Pid_Reset_2();
				}

				SetAng[0] = rcCommand.Ang[0];
				SetAng[1] = rcCommand.Ang[1];

				THROTTLE = rcCommand.Thr;
				X_OUT = 0;   //位置PID输出
				Y_OUT = 0;
				Z_OUT = 0;
				Ang_ctrl();
				break;
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			//光流速度控制
			case QAUD_SPEED:
				if (isFirstAltHold == true)
				{
					THROTTLE = rcCommand.Thr;
					SetXyz[2] = height.height;
					isFirstAltHold = false;
				}
				if (isFirstPosHold == false)
				{
					isFirstPosHold = true;
					pidX->Pid_Reset_2();
					pidY->Pid_Reset_2();
					pidZ->Pid_Reset_2();
					pidU->Pid_Reset_2();
					pidV->Pid_Reset_2();
					pidW->Pid_Reset_2();
				}

				if(rcCommand.Key[1] > 0) osSemaphoreRelease(semEkfIns);
				Speed_ctrl();
				Height_ctrl();
#if 0
///////////////////////////////adding////////////////////////////////////////////////
				//当前角速度
				angleRate << gyro.gyro[0], gyro.gyro[1], gyro.gyro[2];
				angleRate_ << 0.0f, -gyro.gyro[2], gyro.gyro[1],
								  gyro.gyro[2], 0.0f, -gyro.gyro[0],
								  -gyro.gyro[1], gyro.gyro[0], 0.0f;

				F[0] = (J(1,1) - J(2,2)) * angleRate(1) * angleRate(2) / J(0,0);
				F[1] = (J(2,2) - J(0,0)) * angleRate(2) * angleRate(0) / J(1,1);
				F[2] = (J(0,0) - J(1,1)) * angleRate(0) * angleRate(1) / J(2,2);

				time += dt;

				for (int i=0; i<3; i++)
				{
					if(time <= 5) Wnc = 5;
					else Wnc = 15;

					dddWn[i] = -2 * Wb * ddWn_last[i] - 2 * Wb*Wb * dWn_last[i] - Wb*Wb*Wb * Wn[i] + Wb*Wb*Wb * Wnc;
					ddWn[i] = ddWn_last[i] + dddWn[i] * dt;
					dWn[i] = dWn_last[i] + ddWn[i] * dt;
					Wn[i] = Wn_last[i] + dWn[i] * dt;

					beta1[i] = abs(2.0f * Wn[i] - dWn[i] / Wn[i]);
					beta2[i] = abs(Wn[i]*Wn[i] + ddWn[i] / Wn[i] - 2.0f * dWn[i] - ((dWn[i]*dWn[i]) / (Wn[i]*Wn[i])));

					X1_[i] = X1_[i] + h * (F[i] + moments(i)/J(i,i) + X2_[i] + beta1[i] * (angleRate(i) - X1_[i]));
					X2_[i] = X2_[i] + h * beta2[i] * (angleRate(i) - X1_[i]);

					Wn_last[i] = Wn[i];
					dWn_last[i] = dWn[i];
					ddWn_last[i] = ddWn[i];
				}
				D << X2_[0], X2_[1], X2_[2];

				moments = J * D;

				addingmoments.m1 = moments(0);
				addingmoments.m2 = moments(1);
				addingmoments.m3 = moments(2);

				xQueueOverwrite(queueMoments, &addingmoments);

/////////////////////////////////////////////////////////////////////////////////////////
				X_OUT = 0;
				Y_OUT = 0;
#endif
				Ang_ctrl();
				break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////autolifftoff-test////////////////////////////////////////////////////////////////////////////
//			//光流速度控制
//			case QAUD_SPEED:
//				if (isFirstAltHold == true)
//				{
//					isFirstAltHold = false;
//					isLiftOff = false;
//					liftoff_count = 0;
//				}
//				if (isFirstPosHold == false)
//				{
//					isFirstPosHold = true;
//					pidX->Pid_Reset_2();
//					pidY->Pid_Reset_2();
//					pidU->Pid_Reset_2();
//					pidV->Pid_Reset_2();
//				}
//				Speed_ctrl();
//				//----------------rolling-liftoff------------------//
//				//检测起飞
//				if(rcCommand.Key[1] > 0 && isLiftOff == false)	isLiftOff = true;
//				//1s内完成起飞，并定高1m，油门线性增加终值为870，SetXyz线性增加，终值为1
//				if(isLiftOff == true && liftoff_count < 1000)
//				{
//					//起飞后将降落标志位置位
//					if(isFirstTakenOff == false) isFirstTakenOff = true;
//					//油门限幅渐进
//					THROTTLE = PWM_IDLE + liftoff_count * 0.8;
//					SetXyz[2] = setHeight * 0.001f * liftoff_count;
//					liftoff_count ++ ;
//				}
//				//检测降落
//				if(rcCommand.Key[1] > 0 && liftoff_count >= 1000)
//				{
//					isLiftOff = false;
//					//限幅
//					liftoff_count = 1000;
//				}
//				//1s内完成降落，油门线性减小，终值为PWM_IDLE，SetXyz线性递减
//				if(isLiftOff == false && liftoff_count > 0)
//				{
//					//如果是第一次进入降落循环，则记录当前高度
//					if(isFirstTakenOff)
//					{
//						TakenOffHeight = height.height;
//						isFirstTakenOff = false;
//					}
//					//0.8为油门限幅系数，人为根据飞行情况设定
//					THROTTLE = PWM_IDLE + liftoff_count * 0.8;
//					SetXyz[2] = TakenOffHeight * 0.001f * liftoff_count;
//					liftoff_count -- ;
//				}
//				//-------------------------------------------------//
//				Height_ctrl();
//				DAng_ctrl();
//				break;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			case QAUD_PROCESS_POS:
				if(isFirstProcessPos == true)
				{
					isFirstProcessPos = false;
					//将当前位置记录下来，定点悬停
					Xyz_offset[0] = ekf_uwb_pos_vel.Pos[0];
					Xyz_offset[1] = ekf_uwb_pos_vel.Pos[1];
					setHeight = 0.8;
					rolling_liftoff_mode = process_standby;
					pidTHRH->Pid_Set_Para(20.0f, 1.0f, 0.00f, 0.0f, 10000, 4*M_PI, M_PI);
					pidTHR->Pid_Set_Para(34.0f, 7.8f, 0.00f, 0.0f, 10000, 4*M_PI, M_PI);
				}
				//----------------rolling-liftoff------------------//
				switch(rolling_liftoff_mode)
				{
				case process_standby:
					//TakeOff_Count限幅，降落标志位置为
					isFirstLanding = true;
					TakeOff_Count = 1;
					THROTTLE_Offset = 80;
					THROTTLE = 0;
					//检测到遥控变化则切换模式
					if(rcCommand.Key[1] > 0)
					{
						//防抖
						osDelay(20);
						if(rcCommand.Key[1] > 0) rolling_liftoff_mode = process_takeoff;
					}
					pidTHR->Pid_Reset_2();pidTHRH->Pid_Reset_2();pidW->Pid_Reset_2();pidZ->Pid_Reset_2();
					break;
				case process_takeoff:
					//当到达指定高度切换模式且油门减一定值进入PID定高模式
					if(height.height >= setHeight || THROTTLE > 1400)
					{
						rolling_liftoff_mode = process_flying;
						THROTTLE_Offset = THROTTLE;
						Ang_ctrl();
						break;
					}
					FeedBackXyz[2] = height.height;FeedBackUvw[2] = height.speedZ;
					if (SetXyz[2] < setHeight) SetXyz[2] = setHeight * 0.0005 * TakeOff_Count;
					else SetXyz[2] = setHeight;
					SetTHR =  pidTHRH->Pid_Controller(SetXyz[2], FeedBackXyz[2], 0.001, 1, -20.0f, 20.0f);
					THROTTLE = pidTHR->Pid_Controller(SetTHR, FeedBackUvw[2], 0.001, 1, -1400.0f, 1400.0f);
					TakeOff_Count ++;
					Speed_ctrl();
					Ang_ctrl();
					break;
				case process_flying:
					THROTTLE = THROTTLE_Offset;

					SetXyz[2] = setHeight;
					Height_ctrl();
					Speed_ctrl();
					Ang_ctrl();
					//TakeOff_Count限幅
					TakeOff_Count = 5000;
					//检测降落
					if(rcCommand.Key[1] > 0)
					{
						//防抖
						osDelay(20);
						if(rcCommand.Key[1] > 0) rolling_liftoff_mode = process_landing;
					}
					pidTHR->Pid_Reset_2();pidTHRH->Pid_Reset_2();
					break;
				case process_landing:
					if(isFirstLanding == true)
					{
						LandingHeight = height.height;
						isFirstLanding = false;
					}
					FeedBackXyz[2] = height.height;FeedBackUvw[2] = height.speedZ;
					SetXyz[2] = LandingHeight * 0.0002 * TakeOff_Count;
					SetTHR =  pidTHRH->Pid_Controller(SetXyz[2], FeedBackXyz[2], 0.001, 1, -20.0f, 20.0f);
					THROTTLE = THROTTLE_Offset + pidTHR->Pid_Controller(SetTHR, FeedBackUvw[2], 0.001, 1, -1400.0f, 1400.0f);
					TakeOff_Count --;
					Height_ctrl();
					Speed_ctrl();
					Ang_ctrl();
					 if(TakeOff_Count <= 1) rolling_liftoff_mode = process_standby;
					break;
				}
				//----------------rolling-liftoff--------------------//
				break;
			case QAUD_POS_HOLD:
				getTimer_us(&time_now);
				//---------------------------------
				if (isFirstPosHold == true)
				{
					isFirstPosHold = false;
					//记录初始位置，作为轨迹起始点
					lissajous.setOffset(kal_uwb_pos_vel.Pos[0],kal_uwb_pos_vel.Pos[1],height.height);
					yaw_circle.setOffset(kal_uwb_pos_vel.Pos[0],kal_uwb_pos_vel.Pos[1],height.height);
//					lissajous.setOffset(ekf_uwb_pos_vel.Pos[0],ekf_uwb_pos_vel.Pos[1],height.height);
//					yaw_circle.setOffset(ekf_uwb_pos_vel.Pos[0],ekf_uwb_pos_vel.Pos[1],height.height);
					isFirstAltHold = true;

					ekf_uwb_pos_vel.yaw_mag_record = ahrsEuler.Ang[2];
					xQueueOverwrite(queueUwbPosVel,&ekf_uwb_pos_vel);
				}
				//获取莉萨如轨迹坐标
				if(rcCommand.Key[1] > 0)
				{
					if(isFirstTrajectory == true)
					{
						trajectory_starttime = time_now/1000000.f;
						isFirstTrajectory = false;
					}
					trajectory_time = time_now/1000000.f - trajectory_starttime;
//					SetXyz[0] = lissajous.getPos_x(trajectory_time);
//					SetXyz[1] = lissajous.getPos_y(trajectory_time);
//					SetXyz[2] = lissajous.getPos_z(trajectory_time);
					SetXyz[0] = yaw_circle.getPos_x(trajectory_time);
					SetXyz[1] = yaw_circle.getPos_y(trajectory_time);
					SetXyz[2] = yaw_circle.getPos_z(trajectory_time);
				}
				else
				{
					SetXyz[0] = lissajous.getOffset_x();
					SetXyz[1] = lissajous.getOffset_y();
					SetXyz[2] = lissajous.getOffset_z();
				}
				Height_ctrl();
				Pos_ctrl();
				Speed_ctrl();
				Ang_ctrl();
				break;
		}


//		FeedBackDAng[0] = fConstrain(gyro_filter.gyro_filter[0], -0.55555556*M_PI, 0.55555556*M_PI)*R2D;
//		FeedBackDAng[1] = fConstrain(gyro_filter.gyro_filter[1], -0.55555556*M_PI, 0.55555556*M_PI)*R2D;
//		FeedBackDAng[2] = fConstrain(gyro_filter.gyro_filter[2], -0.55555556*M_PI, 0.55555556*M_PI)*R2D;

		FeedBackDAng[0] = gyro_filter.gyro_filter[0]*R2D;
		FeedBackDAng[1] = gyro_filter.gyro_filter[1]*R2D;
		FeedBackDAng[2] = gyro_filter.gyro_filter[2]*R2D;

		pidRolRate->Pid_Controller(SetDAng[0],FeedBackDAng[0],0.001f,1,-700.0f,800.0f);//原case1，case1测试可用
		pidPitRate->Pid_Controller(SetDAng[1],FeedBackDAng[1],0.001f,1,-700.0f,800.0f);
		pidYawRate->Pid_Controller(SetDAng[2],FeedBackDAng[2],0.001f,1,-700.0f,800.0f);

		ROL_OUT = pidRolRate->get_PIDOutput();
		PIT_OUT = pidPitRate->get_PIDOutput();
		YAW_OUT = pidYawRate->get_PIDOutput();

////////////////////////////////////adding转矩控制器//////////////////////////////////////////////////////////////////////
		if(Mode == QAUD_SPEED)
		{
			ROL_OUT += moments[0] / 10.0;
			PIT_OUT += moments[1] / 10.0;
			YAW_OUT += moments[2] / 10.0;
		}

///////////////////////////////////////////////////////////////////////////////////////////////////////////

		motor_Output();
	}
//	else if(control==false)
//	{
//		Z_OUT = 0;
//		THROTTLE = 0;
//		motor_Output();
//	}

	//数据写入
	for(int i=0;i<3;i++)
	{
		control_msg.SetDAng[i] = SetDAng[i];
		control_msg.FeedBackDAng[i] = FeedBackDAng[i];
		control_msg.SetAng[i] = SetAng[i];
		control_msg.FeedBackAng[i] = FeedBackAng[i];
		control_msg.SetUvw[i] = SetUvw[i];
		control_msg.FeedBackUvw[i] = FeedBackUvw[i];
		control_msg.SetXyz[i] = SetXyz[i];
//		control_msg.FeedBackXyz[i] = ekf_uwb_pos_vel.Pos[i];//FeedBackXyz[0];
		control_msg.FeedBackXyz[i] = FeedBackXyz[i];
		control_msg.uwb_origin[i] = uwb.uwbPos[i];
	}
	control_msg.SetTHR = SetTHR;
	control_msg.THROTTLE = THROTTLE;
	control_msg.Z_OUT = Z_OUT;
//	//--test--//
//	//欧拉角度
//	control_msg.FeedBackAng[0] = ahrsEuler.Ang[0]*R2D;
//	control_msg.FeedBackAng[1] = ahrsEuler.Ang[1]*R2D;
//	control_msg.FeedBackAng[2] = ahrsEuler.Ang[2]*R2D;
//	//动捕数据
//	control_msg.FeedBackXyz[0] = mocap.mocapPos[0];
//	control_msg.FeedBackXyz[1] = mocap.mocapPos[1];
//	control_msg.FeedBackXyz[2] = mocap.mocapPos[2];
	xQueueOverwrite(queueControlMsg, &control_msg);
	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;
}

void ATTITUDE_CTRL::Ang_ctrl()
{
	float sinR, cosR, sinP, cosP;
	//------------姿态角控制-------------------//
	//姿态角反馈值
	FeedBackAng[0] = ahrsEuler.Ang[0]*R2D;
	FeedBackAng[1] = ahrsEuler.Ang[1]*R2D;

	pidRol->Pid_Controller(SetAng[0], FeedBackAng[0],0.001f,0);
	pidPit->Pid_Controller(SetAng[1], FeedBackAng[1],0.001f,0);

	if(LockHead == true)		//锁尾，航向控角度
	{
		if(rcCommand.dAng[2]>-10.0f&&rcCommand.dAng[2]<10.0f) //这里表示遥控器航向通道在中位  飞机航向锁死
			SetAng[2] = HoldYaw + ekf_uwb_pos_vel.yaw_cali * R2D;
		else
			SetAng[2] = rcCommand.Ang[2] + HoldYaw + ekf_uwb_pos_vel.yaw_cali * R2D;

		if(rcCommand.Key[1] > 0)
		{
			if(isFirstTrajectory == false) 	SetAng[2] -= yaw_circle.getYaw(trajectory_time);
		}

		if(SetAng[2] > 360.f) SetAng[2] -= 360.f;
		if(SetAng[2] < 0.0f) SetAng[2] += 360.f;

		FeedBackAng[2] = ahrsEuler.Ang[2]*R2D;		//航向角反馈值

		pidYaw->Pid_Controller(SetAng[2], FeedBackAng[2],0.001f,0);

		//欧拉角速度转机体角速度pqr
		sinR = sinf(ahrsEuler.Ang[0]);
		cosR = cosf(ahrsEuler.Ang[0]);
		sinP = sinf(ahrsEuler.Ang[1]);
		cosP = cosf(ahrsEuler.Ang[1]);

		SetDAng[0] =  pidRol->get_PIDOutput()      - pidYaw->get_PIDOutput()*sinP;
		SetDAng[1] =  pidPit->get_PIDOutput()*cosR + pidYaw->get_PIDOutput()*cosP*sinR;
		SetDAng[2] = -pidPit->get_PIDOutput()*sinR + pidYaw->get_PIDOutput()*cosP*cosR;
	}
	else		//不锁尾，航向控角速度
	{
		SetDAng[0] = pidRol->get_PIDOutput();
		SetDAng[1] = pidPit->get_PIDOutput();
		SetDAng[2] = rcCommand.dAng[2] * 1.5;
	}
	//------------姿态角控制-------------------//
}

void ATTITUDE_CTRL::Height_ctrl()
{
	//-------------------高度控制----------------------//
	FeedBackXyz[2] = height.height;  //高度反馈值，数据来源于激光
	{
		SetUvw[2] =  pidZ->Pid_Controller(SetXyz[2], FeedBackXyz[2], 0.001, 1, -100.0f, 100.0f);
		FeedBackUvw[2] = height.speedZ;
	}
	Z_OUT = pidW->Pid_Controller(SetUvw[2], FeedBackUvw[2], 0.001, 1, -400.0f, 400.0f);
	//-------------------高度控制----------------------//
}

void ATTITUDE_CTRL::Speed_ctrl()
{
	//-------------------速度控制----------------------//
	//横滚角用y轴速度控
	if(Mode == QAUD_SPEED || Mode == QAUD_PROCESS_POS)
	{
		SetUvw[0] = - rcCommand.Uvw[1];
		SetUvw[1] = rcCommand.Uvw[0];
	}
	FeedBackUvw[0] = flow.flowy;
	SetAng[0] = - pidU->Pid_Controller(SetUvw[0], FeedBackUvw[0], 0.001, 1, -30.0f, 30.0f);
	//俯仰角用x轴速度控
	FeedBackUvw[1] = flow.flowx;
	SetAng[1] = - pidV->Pid_Controller(SetUvw[1], FeedBackUvw[1], 0.001, 1, -30.0f, 30.0f);
	//-------------------速度控制----------------------//
}


void ATTITUDE_CTRL::Pos_ctrl()
{
	//-------------------位置控制----------------------//
//	FeedBackXyz[0] = ekf_uwb_pos_vel.Pos[0];
//	FeedBackXyz[1] = ekf_uwb_pos_vel.Pos[1];
	FeedBackXyz[0] = kal_uwb_pos_vel.Pos[0];  //x
	FeedBackXyz[1] = kal_uwb_pos_vel.Pos[1];  //y

	//位置限幅
	pos_constrain();
	//遥控控制
	SetXyz[0] += rcCommand.Uvw[0] / 1.5;
	SetXyz[1] += rcCommand.Uvw[1] / 1.5;
	//东北天坐标系
	SetUvw[0] = pidY->Pid_Controller(SetXyz[1], FeedBackXyz[1], 0.001, 1, -20.0f, 20.0f); // y
	SetUvw[1] = pidX->Pid_Controller(SetXyz[0], FeedBackXyz[0], 0.001, 1, -20.0f, 20.0f); // x
	//-------------------位置控制----------------------//
//	//若完成了校准,在带偏航角的圆周运动中，若偏航角发生变化，则位置控制输出量即速度的控制量需要乘上旋转矩阵
//	if(ekf_uwb_pos_vel.yaw_cali != 0)
//	{
//		yaw_R <<  cosf(yaw_circle.getYaw(trajectory_time)),-sinf(yaw_circle.getYaw(trajectory_time)),
//				  sinf(yaw_circle.getYaw(trajectory_time)),cosf(yaw_circle.getYaw(trajectory_time));
//		xyoutput << SetUvw[1],SetUvw[0];
//		xyoutput = yaw_R * xyoutput;
//		SetUvw[1] = xyoutput(0);
//		SetUvw[0] = xyoutput(1);
//	}
}

void ATTITUDE_CTRL::pid_Para_Update()
{
	xQueuePeek(queuePID, &pid_para, 0);
	for(uint8_t i=0;i<12;i++)
	{
		pid[i].Pid_Set_Para(pid_para.kp[i], pid_para.ki[i], pid_para.kd[i]);
	}
}

void ATTITUDE_CTRL::pos_constrain()
{
	for(int i=0;i<3;i++)
	{
		if (pos_old[i] == 0) pos_old[i] = FeedBackXyz[i];
		else if (FeedBackXyz[i] - pos_old[i] > 0.6f) FeedBackXyz[i] = pos_old[i];
		pos_old[i] = FeedBackXyz[i];
	}
}
ATTITUDE_CTRL attitude_ctrl;
extern "C" void attitudeCtrl_main(void *argument)
{
	attitude_ctrl.Attitude_Ctrl_Init();
	osDelay(200);
	for(;;)
	{
		osSemaphoreAcquire(semAttitudeCtrl,0xffffffff);
		attitude_ctrl.Attitude_ctrl_run();
	}
}




/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
